#include "interface.h"
#include "can.h"
#include "dbus.h"

Interface_gimbal_to_chassis gimbal_to_chassis={0};
Interface_arm_to_chassis arm_to_chassis={0};

void dbus_omni_wheel_speed(Interface_arm_to_chassis* arm)
{
	//dbus_ctrl_data.channel.CH[2]=1122;
	arm->Vx = (dbus_ctrl_data.channel.CH[2]-0x400)*4;
	arm->Vy = (dbus_ctrl_data.channel.CH[3]-0x400)*4;
	arm->Vw = (dbus_ctrl_data.channel.rolling_wheel-0x400)*6;
	//Pos = ((float)dbus_ctrl_data.channel.CH[0]-364)*8191/1320;
} 

void interface_callback(CAN_RxHeaderTypeDef* rx_header, uint8_t* data)
{
	if (rx_header->StdId == 0x111)
	{
		gimbal_to_chassis.Vx = data[0] | (data[1] << 8);
		gimbal_to_chassis.Vy = data[2] | (data[3] << 8);
		gimbal_to_chassis.Vw = data[4] | (data[5] << 8);
		gimbal_to_chassis.power_percentage = data[6] | (data[7] << 8);
		//HAL_IWDG_Refresh(&hiwdg);
	}
}

uint8_t interface_chassis_speed_cmd(CAN_HandleTypeDef *hcan,Interface_arm_to_chassis* arm)
{
	uint8_t mode = 0;
	uint8_t data[8] = {0};
	data[0] = arm->Vx;
	data[1] = arm->Vx >> 8 ;
	data[2] = arm->Vy;
	data[3] = arm->Vy >> 8 ;
	data[4] = arm->Vw;
	data[5] = arm->Vw >> 8 ;
	uint32_t CAN_TX_BOX0;
	CAN_TxHeaderTypeDef tx_header;
	tx_header.StdId = 0x111;
	tx_header.IDE = CAN_ID_STD;
	tx_header.RTR = CAN_RTR_DATA;
	tx_header.DLC = 8;
	tx_header.TransmitGlobalTime = DISABLE;
	mode = HAL_CAN_AddTxMessage(hcan, &tx_header, data, &CAN_TX_BOX0);
	return mode;
}

void interface_exe()
{
	dbus_omni_wheel_speed(&arm_to_chassis);
	interface_chassis_speed_cmd(&hcan2,&arm_to_chassis);
}
